home *** CD-ROM | disk | FTP | other *** search
/ Graphics Plus / Graphics Plus.iso / general / modelers / geomview / source.lha / Geomview / src / lib / geometry / point3 / polyint.c < prev    next >
Encoding:
C/C++ Source or Header  |  1992-12-05  |  8.4 KB  |  332 lines
  1. #include <math.h>
  2. #include "point3.h"
  3. #include "transform.h"
  4. #include "ooglutil.h"
  5. #include "polyint.h"
  6.  
  7. static char msg[] = "polyint.c";
  8.  
  9. #define FUDGE 1.e-6
  10.  
  11. #define PT3SUB_XY(a, b, dst) { (dst).x = (a).x - (b).x; \
  12.                    (dst).y = (a).y - (b).y; \
  13.                    (dst).z = 0.0; }
  14. #define PT3LENGTH_XY(a) sqrt((a).x*(a).x + (a).y*(a).y)
  15. #define PT3DOT_XY(a, b) ((a).x*(b).x + (a).y*(b).y)
  16.  
  17. /*
  18.  * Internal routines.  These should not need to be called directly.
  19.  */
  20. static int PolyInt_InBBox(int n_verts, Point3 *verts, float tol);
  21. static float PolyInt_Angle(Point3 *pt1, Point3 *pt2);
  22. static int PolyInt_Origin(int n_verts, Point3 *verts, Point3 *origin);
  23.  
  24. int PolyZInt(int n_verts, Point3 *verts, float tol,
  25.          vvec *ip, vvec *vertices, vvec *edges, vvec *ep)
  26. {
  27.   int i, j, flag;
  28.   int n_oldhits, n_hits = 0;
  29.  
  30.   Point3 v1, v2, pt, bma;
  31.   float d, t, len, f;
  32.  
  33.   float angsum = 0.0;
  34.  
  35.   /* Do test for trivial rejection */
  36.   if (!PolyInt_InBBox(n_verts, verts, tol)) return 0; 
  37.  
  38.   n_hits = n_oldhits = VVCOUNT(*ip);
  39.  
  40.   /* Go through the edges of the polygon looking for edge and vertex hits. */
  41.   for (i = 0; i < n_verts; i++) {
  42.     j = (i + 1) % n_verts;
  43.  
  44.     Pt3From(&v1, (float)verts[i].x, (float)verts[i].y, (float)0.0);
  45.     Pt3From(&v2, (float)verts[j].x, (float)verts[j].y, (float)0.0);
  46.  
  47.     Pt3Sub(&verts[j], &verts[i], &bma);
  48.     len = PT3LENGTH_XY(bma);
  49.     t = - PT3DOT_XY(verts[i], bma) / len;      
  50.     
  51.     f = t / len;
  52.     Pt3From(&pt, (float)(verts[i].x + f * bma.x), 
  53.         (float)(verts[i].y + f * bma.y), (float)(verts[i].z + f * bma.z));
  54.     d = PT3LENGTH_XY(pt);
  55.  
  56.     if (d < tol && t > -tol) {
  57.       if (t < tol) {
  58.     *VVINDEX(*vertices, int, n_hits) = i;
  59.     *VVINDEX(*edges, int, n_hits) = -1;
  60.     *VVINDEX(*ip, Point3, n_hits) = verts[i];
  61.     n_hits++;
  62.       } else if (t < len - tol) {
  63.     *VVINDEX(*ip, Point3, n_hits) = pt;
  64.     *VVINDEX(*vertices, int, n_hits) = -1;
  65.     *VVINDEX(*edges, int, n_hits) = i;
  66.     *VVINDEX(*ep, Point3, n_hits) = pt;
  67.     n_hits++;
  68.       }
  69.     }
  70.  
  71.     angsum += PolyInt_Angle(&v1, &v2);
  72.  
  73.   }
  74.  
  75.   /* Look for the face hits */
  76.   if (!(n_hits - n_oldhits) && n_verts > 2 && fabs(angsum / M_2_PI) > 0.5
  77.     && PolyInt_Origin(n_verts, verts, &pt)) {
  78.     *VVINDEX(*ip, Point3, n_hits) = pt;
  79.     *VVINDEX(*vertices, int, n_hits) = -1;
  80.     *VVINDEX(*edges, int, n_hits) = -1;
  81.     n_hits++;
  82.   }
  83.  
  84.   VVCOUNT(*ip) = n_hits;
  85.   VVCOUNT(*vertices) = n_hits;
  86.   VVCOUNT(*edges) = n_hits;
  87.  
  88.   return(n_hits - n_oldhits);
  89.  
  90. }
  91.  
  92.  
  93. int PolyNearPosZInt(int n_verts, Point3 *verts, float tol,
  94.             Point3 *ip, int *vertex, int *edge, Point3 *ep) {
  95.   int i, closest;
  96.   vvec v_ip, v_vertices, v_edges, v_ep;
  97.   float zmax;
  98.  
  99.   VVINIT(v_ip, Point3, 4);
  100.   VVINIT(v_vertices, int, 4);
  101.   VVINIT(v_edges, int, 4);
  102.   VVINIT(v_ep, Point3, 4);
  103.  
  104.   closest = -1;
  105.  
  106.   if (!PolyZInt(n_verts, verts, tol, &v_ip, &v_vertices, &v_edges, &v_ep))
  107.     return 0;
  108.  
  109.   for (i = 0; i < VVCOUNT(v_ip); i++) 
  110.     if (VVINDEX(v_ip, Point3, i)->z > -1.0 && 
  111.     (closest == -1 || VVINDEX(v_ip, Point3, i)->z > zmax)) {
  112.       closest = i;
  113.       zmax = VVINDEX(v_ip, Point3, i)->z;
  114.     }
  115.  
  116.   if (closest >= 0) {
  117.     Pt3Copy(VVINDEX(v_ip, Point3, closest), ip);
  118.     *vertex = *VVINDEX(v_vertices, int, closest);
  119.     *edge = *VVINDEX(v_edges, int, closest);
  120.     Pt3Copy(VVINDEX(v_ep, Point3, closest), ep);
  121.   }
  122.  
  123.   vvfree(&v_ip);
  124.   vvfree(&v_vertices);
  125.   vvfree(&v_edges);
  126.   vvfree(&v_ep);
  127.  
  128.   return ((closest < 0) ? 0 : 1);
  129. }
  130.     
  131.  
  132. int PolyLineInt(Point3 *pt1, Point3 *pt2, int n_verts, Point3 *verts, 
  133.         float tol, vvec *ip, vvec *vertices, vvec *edges, vvec *ep) 
  134. {
  135.   int i;
  136.   int n_hits, old_n_hits;
  137.   Point3 *vertcopy;
  138.   Transform T, Tinv;
  139.  
  140.   PolyInt_Align(pt1, pt2, T);
  141.   TmInvert(T, Tinv);
  142.  
  143.   vertcopy = OOGLNewNE(Point3, n_verts, msg);
  144.   for (i = 0; i < n_verts; i++) Pt3Transform(T, &verts[i], &vertcopy[i]);
  145.  
  146.   old_n_hits = VVCOUNT(*ip);
  147.   n_hits = PolyZInt(n_verts, vertcopy, tol, ip, vertices, edges, ep);
  148.   for (i = 0; i < n_hits; i++) {
  149.     Pt3Transform(Tinv, VVINDEX(*ip, Point3, old_n_hits + i), 
  150.          VVINDEX(*ip, Point3, old_n_hits + i));
  151.     Pt3Transform(Tinv, VVINDEX(*ep, Point3, old_n_hits + i),
  152.          VVINDEX(*ep, Point3, old_n_hits + i));
  153.   }
  154.  
  155.   OOGLFree(vertcopy);
  156.  
  157.   return n_hits;
  158.  
  159. }
  160.  
  161.  
  162. int PolyRayInt(Point3 *pt1, Point3 *pt2, int n_verts, Point3 *verts, 
  163.            float tol, vvec *ip, vvec *vertices, vvec *edges, vvec *ep)
  164. {
  165.   int i, j;
  166.   int n_hits, old_n_hits;
  167.   Point3 *vertcopy;
  168.   Transform T, Tinv;
  169.  
  170.   PolyInt_Align(pt1, pt2, T);
  171.   TmInvert(T, Tinv);
  172.  
  173.   vertcopy = OOGLNewNE(Point3, n_verts, msg);
  174.   for (i = 0; i < n_verts; i++) Pt3Transform(T, &verts[i], &vertcopy[i]);
  175.  
  176.   old_n_hits = VVCOUNT(*ip);
  177.   n_hits = PolyZInt(n_verts, vertcopy, tol, ip, vertices, edges, ep);
  178.   for (i = j = 0; i < n_hits; i++)
  179.     if (VVINDEX(*ip, Point3, old_n_hits + i)->z <= 0) {
  180.       Pt3Transform(Tinv, VVINDEX(*ip, Point3, old_n_hits + i),
  181.            VVINDEX(*ip, Point3, old_n_hits + j));
  182.       Pt3Transform(Tinv, VVINDEX(*ep, Point3, old_n_hits + i),
  183.            VVINDEX(*ep, Point3, old_n_hits + j));
  184.       j++;
  185.     }
  186.  
  187.   OOGLFree(vertcopy);
  188.  
  189.   return j;
  190.  
  191. }
  192.  
  193.  
  194. int PolySegmentInt(Point3 *pt1, Point3 *pt2, int n_verts, Point3 *verts,
  195.            float tol, vvec *ip, vvec *vertices, vvec *edges, vvec *ep)
  196. {
  197.   int i, j;
  198.   int n_hits, old_n_hits;
  199.   Point3 *vertcopy;
  200.   Transform T, Tinv;
  201.  
  202.   PolyInt_Align(pt1, pt2, T);
  203.   TmInvert(T, Tinv);
  204.  
  205.   vertcopy = OOGLNewNE(Point3, n_verts, msg);
  206.   for (i = 0; i < n_verts; i++) Pt3Transform(T, &verts[i], &vertcopy[i]);
  207.  
  208.   old_n_hits = VVCOUNT(*ip);
  209.   n_hits = PolyZInt(n_verts, vertcopy, tol, ip, vertices, edges, ep);
  210.   for (i = j = 0; i < n_hits; i++) 
  211.     if ((VVINDEX(*ip, Point3, old_n_hits + i)->z <= 0) &&
  212.     (VVINDEX(*ip, Point3, old_n_hits + i)->z >= -1.0)) {
  213.       Pt3Transform(Tinv, VVINDEX(*ip, Point3, old_n_hits + i),
  214.            VVINDEX(*ip, Point3, old_n_hits + j));
  215.       Pt3Transform(Tinv, VVINDEX(*ep, Point3, old_n_hits + i),
  216.            VVINDEX(*ep, Point3, old_n_hits + j));
  217.       j++;
  218.     }
  219.  
  220.   OOGLFree(vertcopy);
  221.  
  222.   VVCOUNT(*ip) = old_n_hits + j;
  223.   vvtrim(ip);
  224.  
  225.   return j;
  226.  
  227. }
  228.  
  229.  
  230. void PolyInt_Align(Point3 *pt1, Point3 *pt2, Transform T) {
  231.   Point3 newpt2;
  232.   Transform Ttmp;
  233.  
  234.   if (!bcmp(pt1, pt2, sizeof(Point3))) {
  235.     OOGLError(1, "PolyInt_Align called with identical points.");
  236.     TmIdentity(T);
  237.     return;
  238.   }
  239.  
  240.   TmTranslate(T, -pt1->x, -pt1->y, -pt1->z);
  241.   Pt3Transform(T, pt2, &newpt2);
  242.  
  243.   TmRotateY(Ttmp, -atan2(newpt2.x, -newpt2.z));
  244.   TmConcat(T, Ttmp, T);
  245.   Pt3Transform(T, pt2, &newpt2);
  246.  
  247.   TmRotateX(Ttmp, -atan2(newpt2.y, -newpt2.z));
  248.   TmConcat(T, Ttmp, T);
  249.   Pt3Transform(T, pt2, &newpt2);
  250.  
  251.   if (newpt2.z == 0.0) {
  252.     OOGLError(1, "Second point too close to first point in PolyInt_Align");
  253.     TmIdentity(T);
  254.     return;
  255.   }
  256.   TmScale(Ttmp, -1.0 / newpt2.z, -1.0 / newpt2.z, -1.0 / newpt2.z);
  257.   TmConcat(T, Ttmp, T);
  258.  
  259. }
  260.  
  261. /*
  262.  * PolyInt_InBBox
  263.  * Returns non-zero if the origin is inside the polygon described by
  264.  * verts or within tol of begin so; returns 0 otherwise.
  265.  */
  266. static int PolyInt_InBBox(int n_verts, Point3 *verts, float tol) {
  267.   int i;
  268.   int posx = 0, negx = 0, posy = 0, negy = 0;
  269.  
  270.   for (i = 0; i < n_verts; i++, verts++) {
  271.     if (verts->x < tol) negx |= 1;
  272.     if (verts->x > -tol) posx |= 1;
  273.     if (verts->y < tol) negy |= 1;
  274.     if (verts->y > -tol) posy |= 1;
  275.   }
  276.  
  277.   return (posx & negx & posy & negy);
  278.  
  279. }
  280.  
  281.  
  282. static float PolyInt_Angle(Point3 *pt1, Point3 *pt2) {
  283.   float ans;
  284.  
  285.   if (Pt3Distance(pt1, pt2) < FUDGE) return 0;
  286.   ans = PT3DOT_XY(*pt1, *pt2) / 
  287.     sqrt((double)(PT3DOT_XY(*pt1, *pt1) * PT3DOT_XY(*pt2, *pt2)));
  288.   ans = acos(ans);
  289.   return(ans * ((pt1->x * pt2->y - pt1->y * pt2->x) >= 0 ? 1.0 : -1.0));
  290.  
  291. }
  292.  
  293.  
  294. static int PolyInt_Origin(int n_verts, Point3 *verts, Point3 *origin) {
  295.   int bi, ci;
  296.   float pz, pw;
  297.  
  298.   /* Find the first vertex different from the 0th vertex */
  299.   for (bi = 0; bi < n_verts && !bcmp(&verts[0], &verts[bi], sizeof(Point3));
  300.        bi++);
  301.   if (bi >= n_verts) {
  302.     Pt3Copy(&verts[0], origin);
  303.     return 0;
  304.   }
  305.  
  306.   /* Find the first vertex not collinear with the other two vertices */
  307.   for (ci = bi+1; ci < n_verts; ci++) {
  308.     pz = (verts[0].x * (verts[bi].y - verts[ci].y) 
  309.       - verts[0].y * (verts[bi].x - verts[ci].x) 
  310.       + (verts[bi].x * verts[ci].y - verts[bi].y * verts[ci].x));
  311.     if (fabs(pz) > FUDGE) break;
  312.   }
  313.   if (ci >= n_verts) {
  314.     Pt3Copy(&verts[0], origin);
  315.     return 0;
  316.   }
  317.  
  318.   pw = (verts[0].x * (verts[bi].z*verts[ci].y - verts[bi].y*verts[ci].z)
  319.     - verts[0].y * (verts[bi].z*verts[ci].x - verts[bi].x*verts[ci].z)
  320.     + verts[0].z * (verts[bi].y*verts[ci].x - verts[bi].x*verts[ci].y));
  321.  
  322.   origin->x = origin->y = 0.0;
  323.   origin->z = -pw / pz;
  324.  
  325.   return 1;
  326. }
  327.  
  328.  
  329.  
  330.  
  331.  
  332.